package org.opentcs.strategies.basic.dispatching.rerouting;

import jakarta.inject.Inject;
import org.opentcs.components.kernel.Router;
import org.opentcs.data.model.Point;
import org.opentcs.data.order.ReroutingType;
import org.opentcs.data.order.Route;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

import java.util.ArrayList;
import java.util.List;
import java.util.Objects;

public class DyDriveOrderMerger extends AbstractDriveOrderMerger {

  private static final Logger LOG = LoggerFactory.getLogger(RegularDriveOrderMerger.class);

  /**
   * Creates a new instance.
   *
   * @param router The router to use.
   */
  @Inject
  public DyDriveOrderMerger(Router router) {
    super(router);
  }

  @Override
  protected List<Route.Step> mergeSteps(List<Route.Step> oldStepsA,
                                        List<Route.Step> newStepsB,
                                        int currentRouteStepIndex) {
    LOG.debug("Merging steps {} with {}", stepsToPaths(oldStepsA), stepsToPaths(newStepsB));
    List<Route.Step> mergedSteps = new ArrayList<>();

    // Get the step where stepsB starts to diverge from stepsA (i.e. the step where routeA and
    // routeB share the same source point).
    Route.Step divergingStep = findStepWithSource(newStepsB.get(0).getSourcePoint(), oldStepsA, currentRouteStepIndex);
    int divergingIndex = oldStepsA.indexOf(divergingStep);
    mergedSteps.addAll(oldStepsA.subList(0, divergingIndex));

    // Set the rerouting type for the first step in the new route.
    Route.Step firstStepOfNewRoute = newStepsB.get(0);
    List<Route.Step> modifiedStepsB = new ArrayList<>(newStepsB);
    modifiedStepsB.set(0, new Route.Step(firstStepOfNewRoute.getPath(),
        firstStepOfNewRoute.getSourcePoint(),
        firstStepOfNewRoute.getDestinationPoint(),
        firstStepOfNewRoute.getVehicleOrientation(),
        firstStepOfNewRoute.getRouteIndex(),
        firstStepOfNewRoute.isExecutionAllowed(),
        ReroutingType.REGULAR));

    mergedSteps.addAll(modifiedStepsB);

    // Update the steps route indices since they originate from two different drive orders.
    mergedSteps = updateRouteIndices(mergedSteps);

    return mergedSteps;
  }

  private Route.Step findStepWithSource(Point sourcePoint, List<Route.Step> steps, int index) {
    LOG.debug("Looking for a step with source point {} in {}",
        sourcePoint,
        stepsToPaths(steps));
    //避免路径中有重复经过一个点，导致重路由路径拼接不正常。所以重路由的几点一定是在index之后的或者就是index
    LOG.info("合并路由，执行到了路由下小标为:{}", index);
    int skipNum = 0, j = 0;
    for (Route.Step step1 : steps) {
      if (step1.getReroutingType() != null)
        skipNum = j;
      j++;
    }
    LOG.info("合并路由，老重路由执行到的index:{}", skipNum);
    return steps.stream().skip(skipNum)
        .filter(step -> Objects.equals(step.getSourcePoint(), sourcePoint) && step.getRouteIndex() >= index)
        .findFirst().get();
  }
}
